/*
 * File:    pwm.c
 */

#include <p24FJ64GA002.h>
#include "pwm.h"

/** ************************************************************************ **/

void InitializePWM(void)
{
    /* ... outputs ...
    TRISBbits.TRISB10 = 0; Chip Enable
    TRISBbits.TRISB9 = 0; Wheel 1
    TRISBbits.TRISB8 = 0; Wheel 1
    TRISBbits.TRISB6 = 0; Wheel 2
    TRISBbits.TRISB5 = 0; Wheel 2 */
    TRISB &= 0xF89F;

    /* ... that are initially all set low.
    LATBbits.LATB10 = 0;
    LATBbits.LATB9 = 0;
    LATBbits.LATB8 = 0;
    LATBbits.LATB6 = 0;
    LATBbits.LATB5 = 0; */
    LATB &= 0xF89F;

    /* Set the initial configurations for the output compare pins. */
    /* Set RB14 as Output Compare 1 */
    /* Note: for Output Compare 2 set to 0x13 */
    _RP9R = 0x12;
    _RP6R = 0x12;

    /* Configure Timer2 with a prescaler of 1:1, 32-bit mode operation off,
     * no idle mode device operation, and initially off.
     *
     * DONE_TODO: Solve weird ass bug---OC doesn't work with anything other than
     * a prescaler of 1:1. WTF, mate?
     * Update: When using a prescaler that doesn't equal 1:1, setting OC1R and
     * OC1RS to the same value prohibits the pulse from driving the motor. This
     * is due to the fact that OC1R stores the time the signal goes from low to
     * high, while OC1RS stores it from high to low. So, in actuality, this
     * makes perfect sense. */
    T2CON = 0x2020;

    /* Reset timer counter value */
    TMR2 = 0;

    /* Start with a 20Hz frequency output from Timer2 */
    PR2 = 5760;

    /* Configure the output compare module with a disabled output channel,
     * Timer2 source and no operation in idle mode. */
OC1CON |= 0x2000;
    OC2CON |= 0x2000;

    /* Initially configure OC1 to generate a 50% duty cycle, so we should get
     * half of the theoretical maximum output from the motor.
     *
     * Note OC1R stores the value at which the output goes high (low to high),
     * while OC1RS stores the value at which OC1 goes low (high to low). We can
     * change the speed at which the motor turns by reducing the value in OC1R.
     *
     * At about OC1R = 4700, the motor stops moving, and only RUMBLES!!
     */
    OC1R = 5760;
    OC1RS = 5760;

    OC2R = 5760;
    OC2RS = 5760;
}

void StartPWM(void)
{
    /* Set Chip Enable 1 */
    LATBbits.LATB10 = 1;

    /* Set Timer2 on to enable output compare modules */
    T2CONbits.TON = 1;
}

void Stop(void)
{
    /* Disable output channel */
    OC1CON &= 0xFFF8;
    OC2CON &= 0xFFF8;
}

void Resume(void)
{
    OC1CON |= 0x5;
    OC2CON |= 0x5;
}

void PivotCCW(void)
{
    _RP8R = 0;
    _RP9R = 0x12;

    _RP5R = 0x12;
    _RP6R = 0;

    /* Set OC1 to generate continuous pulses. */
    OC1CON |= 0x5;
    OC2CON |= 0x5;
}

void PivotCW(void)
{
/* Use the same pulse for both reverse and forward and just turn down speed
 * linearly */
    _RP8R = 0x12;
    _RP9R = 0;

    _RP5R = 0;
    _RP6R = 0x12;

    /* Set OC1 to generate continuous pulses. */
    OC1CON |= 0x5;
    OC2CON |= 0x5;
}

void Forward(void)
{
/* Note for this state \ is the only state that allows the two wheels to be set
 * to different pulse channels.  This is to allow the car to turn by slowing
 * down one wheel's pulse */

    _RP9R = 0x12;
    _RP8R = 0;

    _RP6R = 0x13;
    _RP5R = 0;
    OC1CON |= 0x5;
    OC2CON |= 0x5;
}